package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.templates.commands.elevatorDown;
import edu.wpi.first.wpilibj.templates.commands.elevatorMiddle;
import edu.wpi.first.wpilibj.templates.commands.elevatorUp;
import edu.wpi.first.wpilibj.templates.commands.emergencyServo;
import edu.wpi.first.wpilibj.templates.commands.endgameservo;
import edu.wpi.first.wpilibj.templates.commands.liftDown;
import edu.wpi.first.wpilibj.templates.commands.liftUp;
import edu.wpi.first.wpilibj.templates.commands.loaderservo;
import edu.wpi.first.wpilibj.templates.commands.teleLoad;
import edu.wpi.first.wpilibj.templates.commands.teleShoot;

/**
 * This class is the glue that binds the controls on the physical operator
 * interface to the commands and command groups that allow control of the robot.
 */
public class OI {
    
    public static Joystick stick= new Joystick(RobotMap.joystick_port);
    
//    public final Button shooter_enable = new JoystickButton(stick,RobotMap.shooter_button);
    public final Button aim_enable = new JoystickButton(stick,RobotMap.aim_button);
    
    public final Button load_button = new JoystickButton(stick,RobotMap.load_button);
    public final Button shoot_button = new JoystickButton(stick,RobotMap.shoot_button);
    public final Button elevator_up = new JoystickButton(stick,RobotMap.elevator_up);   
    public final Button elevator_middle=new JoystickButton(stick,RobotMap.elevator_middle);
    public final Button elevator_down = new JoystickButton(stick,RobotMap.elevator_down);    
    public final Button pickUp_button = new JoystickButton(stick,RobotMap.pickUp_buton);
//    public final Button shiftUp_button = new JoystickButton(stick,RobotMap.shift_up);
//    public final Button shiftDown_button = new JoystickButton(stick,RobotMap.shift_down);
    public final Button lift_up = new JoystickButton(stick,RobotMap.lifter_up);
    public final Button lift_down = new JoystickButton(stick,RobotMap.lifter_down);
    public final Button end_ears = new JoystickButton(stick,RobotMap.endGame_release);
    public final Button emergencyEnd = new JoystickButton(stick,RobotMap.emergency_end);
    public final Button loader_open = new JoystickButton(stick, RobotMap.loader_open);
    
  
    
        
    public Joystick getStick()
    {
        return stick;
    }
    public double getStickX()
    {
        return stick.getX();
    }
    public double getStickY()
    {
        return stick.getY();
    }
    public double getStickZ()
    {
        return stick.getZ();
    }
    public double getAxisY()
    {
        return stick.getAxis(Joystick.AxisType.kY);
    }        
    public int elevator_state = 0; // 0 below the middle switch, 1 is above middle switch
    public double elevator_speed = 1;
    public int elevator_last_direction = 0; //0=stopped, 1=up, -1=down;
    public int drive_state = 0; //0=manual, 1=automatic;
////    aim obj_aim = new aim();

    public OI (){
//        shooter_enable.whileHeld(new teleShoot());
        lift_up.whileHeld(new liftUp());
        lift_down.whileHeld(new liftDown());
//        aim_enable.whenPressed(new aim());
//        aim_enable.whenPressed(new aimWithDrive());
        shoot_button.whenPressed(new teleShoot());
        elevator_up.whileHeld(new elevatorUp());
        loader_open.whenPressed(new elevatorMiddle());
        elevator_down.whileHeld(new elevatorDown());
//        shiftUp_button.whileHeld(new UpShift());
//        shiftDown_button.whileHeld(new DownShift());
        end_ears.whenPressed(new endgameservo());
        emergencyEnd.whenPressed(new emergencyServo());
        loader_open.whileHeld(new teleLoad());
        
        
//        while (true)
//        {
//            System.out.println("Upper"+RobotMap.liftUpLimit.get());
//            System.out.println("Lower"+RobotMap.liftDownLimit.get());
//        }
    }
    //// CREATING BUTTONS
    // One type of button is a joystick button which is any button on a joystick.
    // You create one by telling it which joystick it's on and which button
    // number it is.
    // Joystick stick = new Joystick(port);
    // Button button = new JoystickButton(stick, buttonNumber);
    
    // Another type of button you can create is a DigitalIOButton, which is
    // a button or switch hooked up to the cypress module. These are useful if
    // you want to build a customized operator interface.
    // Button button = new DigitalIOButton(1);
    
    // There are a few additional built in buttons you can use. Additionally,
    // by subclassing Button you can create custom triggers and bind those to
    // commands the same as any other Button.
    
    //// TRIGGERING COMMANDS WITH BUTTONS
    // Once you have a button, it's trivial to bind it to a button in one of
    // three ways:
    
    // Start the command when the button is pressed and let it run the command
    // until it is finished as determined by it's isFinished method.
    // button.whenPressed(new ExampleCommand());
    
    // Run the command while the button is being held down and interrupt it once
    // the button is released.
    // button.whileHeld(new ExampleCommand());
    
    // Start the command when the button is released  and let it run the command
    // until it is finished as determined by it's isFinished method.
    // button.whenReleased(new ExampleCommand());
}

